//
//                 Robot Control Program
//
#include <stdio.h>
#include <cmn\userLib.h>
#include <a2d\a2d.h>
#include <pwm\pwm.h>
#include <pwm\vexMotor.h>
#include <cmn\flash.h>
#include <rfinder\rfinder_TS601P01.h>
#include <lib_usb_cdc.h>
#include "my_main.h"

UINT16 vex1Handle;		// vex left motor
UINT16 vex2Handle;

void my_main(void)
{
	int x = 0;
	INT8	val8;
	INT16	speed = 0;
	INT16	speedIncr = 0x10;
	int i;
	int rc;
	UINT16 value = 0x00;
	char	buf[40];
	
// This code gives an example of digital pin input and output

	PORTSetPinsDigitalIn( IOPORT_B, BIT_5 );	// set pin RB5 as an input pin
	PORTSetPinsDigitalOut( IOPORT_B, BIT_9 );	// set pin RB9 as an output pin
	PORTClearBits( IOPORT_B, BIT_9 );			// set pin RB9 to 0 (0 volts)
	_TRISB5 = 1;
	ledOut(0);
	if( PORTReadBits( IOPORT_B, BIT_5 ) > 0 )	// read pin RB5 into bit 5 of value
	{
		PORTSetBits( IOPORT_B, BIT_9 );			// set pin RB9 to 1 (+3.3V volts)
		ledOut(1);
	}	
	delayT1_ms(5000);					// wait for 5000 milliseconds = 5 seconds

// test Analog to Digital input, displaying value on UART, then read 
	while(1)
	{
		rc = A2D_read(0, &value);		// reads analog to digital channel 0 into variable value
		checkrc(rc);					// checks return code from previous instruction
		sprintf( buf, "value = %d\r", value );			// format a character string to display value
		printf(buf);									// writes string to UART

		value = Read_Rfinder_TS601P01(IOPORT_B, BIT_7);	// reads rangefinder on pin RB7, placing cm distance into value 
		sprintf( buf, "value = %d\r", value );			// format a character string to display value
#ifdef USE_USB
		lib_usb_cdc_write_blocked( buf, strlen(buf) );	// writes string to USB port
		lib_usb_cdc_process_main();		// service usb - call once per main loop iteration
#endif
		delayT1_ms(100);								// waits 1/10th of a second
		value = value >> 6;								// display only high-order 4 bits
		ledOut(value);									// writes value to LEDs
	}	

// Vex Motor Test
/*	
	while(1)				
	{
		value = speed >> 4;							// display only high-order 4 bits
		ledOut( value );							// writes value to LEDs
		rc = VexMotorWrite( vex1Handle, speed );	// set motor speed
		checkrc(rc);								// checks return code from previous instruction

		speed += speedIncr;							// change speed up/down
		if( speed > 127 )
		{
			speed = 127;
			speedIncr = -0x10;
		}
		if( speed < -127 )
		{
			speed = -127;
			speedIncr = 0x10;
		}
		delayT1_ms(3000);							// speed set for 3000 millisec = 3 sec
	}
*/

// this section demontrates the flash memory read / write	
/*	
	rc = FlashRead( 0, 1, &val8 );
	checkrc(rc);

	if( val8 < 'A' )
		val8 = 'A';
	LATE = val8;
	delayT1_ms(3000);
	++val8;
	rc = FlashWrite( 0, 1, &val8 );			// displayed val will incr w/ each power cycle
	checkrc(rc);
*/	
/*
	PWMWrite( pwm1Handle, PR2/4 );	// PWM write test
	while(1)
	{}
*/

/*
   	while(1)		// 541 Test
    {
		MAX541Write( max541Handle, value );
		LATE = value >> 8;
		delayT1_ms(1);		
		++value;
		
	}
*/
	

} // main
